cmake_minimum_required(VERSION 3.8)
project(2d_slam)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io filters)
find_package(glog REQUIRED)
find_package(gflags REQUIRED)
find_package(rosbag2_cpp REQUIRED)
find_package(rosbag2_storage REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(std_msgs REQUIRED)

set(g2o_DIR "/usr/local/lib/cmake/g2o")
find_package(g2o REQUIRED)
if(g2o_FOUND)
  message(STATUS "g2o found: ${g2o_INCLUDE_DIRS}")
else()
  message(FATAL_ERROR "g2o not found!")
endif()



include_directories(
  ${CMAKE_CURRENT_SOURCE_DIR}/src
  ${EIGEN3_INCLUDE_DIRS}
  /opt/ros/humble/include  # 添加 Sophus 头文件路径
  /src/2d_slam/src/sophus
)

# 2D SLAM Library
add_library(${PROJECT_NAME} SHARED
  src/ch4/imu_preintegration.cc
  src/ch6/icp_2d.cc
  src/ch6/lidar_2d_utils.cc
  src/ch6/likelihood_field.cc
  src/ch6/occupancy_map.cc
  src/ch6/submap.cc
  src/ch6/mapping_2d.cc
  src/ch6/multi_resolution_likelihood_field.cc
  src/ch6/loop_closing.cc
  src/ch6/frame.cc
  # src/common/io_utils.cc
  src/common/timer/timer.cc
  src/common/global_flags.cc
  src/common/g2o_types.cc
  src/common/point_cloud_utils.cc
)

target_link_libraries(${PROJECT_NAME}
  ${PCL_LIBRARIES}
  glog gflags
  ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_FILTERS_LIBRARIES}
  g2o_core
  g2o_stuff
  g2o_types_slam3d
  g2o_types_slam2d
  g2o_solver_cholmod
)

ament_target_dependencies(${PROJECT_NAME}
  rclcpp
  sensor_msgs
  OpenCV
  Eigen3
  PCL
  glog
  gflags
  g2o
  std_msgs
  pcl_conversions
  rosbag2_cpp
  rosbag2_storage
)


add_executable(ros2_2d_mapping src/ch6/ros2_2d_mapping.cc)
# target_link_libraries(ros2_2d_mapping Eigen3::Eigen)
target_link_libraries(ros2_2d_mapping 
    ${PROJECT_NAME} 
    Eigen3::Eigen
    ${PCL_LIBRARIES}
)
ament_target_dependencies(ros2_2d_mapping rclcpp sensor_msgs OpenCV PCL glog gflags g2o Eigen3 pcl_conversions)

# Installation
install(TARGETS
  ros2_2d_mapping
  DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  set(ament_cmake_copyright_FOUND TRUE)
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()
